#include <iostream>
#include <stdexcept>

#include "kalman.h"

KalmanFilterSimple::KalmanFilterSimple(
    double dt,
    const Eigen::MatrixXd &A,
    const Eigen::MatrixXd &C,
    const Eigen::MatrixXd &Q,
    const Eigen::MatrixXd &R,
    const Eigen::MatrixXd &P)
    : A(A), C(C), Q(Q), R(R), P0(P),
      m(C.rows()), n(A.rows()), dt(dt), initialized(false),
      I(n, n), x_hat(n), x_hat_new(n)
{
    I.setIdentity();
}

KalmanFilterSimple::KalmanFilterSimple() {}

void KalmanFilterSimple::init(double t0, const Eigen::VectorXd &x0)
{
    x_hat = x0;
    P = P0;
    this->t0 = t0;
    t = t0;
    initialized = true;
}

void KalmanFilterSimple::init()
{
    x_hat.setZero();
    P = P0;
    t0 = 0;
    t = t0;
    initialized = true;
}

void KalmanFilterSimple::update(const Eigen::VectorXd &y)
{

    if (!initialized)
        throw std::runtime_error("Filter is not initialized!");
    // std::cout << "P:" << P << std::endl;
    // std::cout << "A:" << A << std::endl;
    // std::cout << "C:" << C << std::endl;
    // std::cout << "Q:" << Q << std::endl;
    // std::cout << "x_hat:" << x_hat << std::endl;

    x_hat_new = A * x_hat;
    P = A * P * A.transpose() + Q;
    K = P * C.transpose() * (C * P * C.transpose() + R).inverse();

    x_hat_new += K * (y - C * x_hat_new);
    P = (I - K * C) * P;
    x_hat = x_hat_new;
    // std::cout << "P1:" << P << std::endl;
    // std::cout << "x_hat_new:" << x_hat_new << std::endl;
    t += dt;
}

void KalmanFilterSimple::changeStates(const Eigen::VectorXd &new_states)
{

    if (!initialized)
        throw std::runtime_error("Filter is not initialized!");
    if (x_hat.size() != new_states.size())
        throw std::runtime_error("State vectors do not have the same size");
    x_hat = new_states;
}

void KalmanFilterSimple::update(const Eigen::VectorXd &y, double dt, const Eigen::MatrixXd A)
{

    this->A = A;
    this->dt = dt;
    update(y);
}

void KalmanFilterSimple::update(const Eigen::VectorXd &y, double dt)
{

    this->dt = dt;
    update(y);
}
